#include "H264StreamPubSubTypes.h"
#include "ObjectBytePubSubTypes.h"
#include "shm_participant.h"
#include "default_subscriber.h"
//#include "geometry_msgs/msgs/Position.h"
// #include "position_sub_callback.h"
//#include "DDSPIM.h"
#include "DDS_PIM.h"
#include <thread>
#include <functional>
#include "stdio.h"
#include <iomanip>

/*
class Msg
{
public:
          void threadFunction(vis_msg::ObjectByte msg)
          {
                std::cout <<"id1:" << msg.id() << "---"<<msg.name()<<" --- "<<msg.channelNumber() <<std::endl;  
                //std::cout <<"okokokokok"<<std::endl;
          }

                    void threadFunction2(vis_msg::H264Stream msg)
          {
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
                std::cout <<"id2:" << msg.id() << "---"<<msg.name()<<" --- "<<msg.channelNumber() <<std::endl;  
                //std::cout <<"okokokokok"<<std::endl;
          }

    //       void start(){
    //         myThread = std::thread(std::bind(&Msg::setMsgCallback, this, std::placeholders::_1),std::bind(&Msg::threadFunction, this, std::placeholders::_1));
    // }
          // void setMsgCallback(std::function<void (vis_msg::H264Stream msg)> callback)
          // {
          //     std::function<void(vis_msg::H264Stream &msg)> mDoMsg = callback;
          //     ShmParticipant dp2;
          //     dp2.init(AODS_I7_MAIN_0, "hi");
          //     vis_msg::H264Stream h264Stream2{};
          //     DDSSubscriber h264_sub2(vis_msg::H264StreamPubSubType(), &h264Stream2, AODS_1126_OBJ_L,dp2.participant());
          //     for (;;) {
          //     h264_sub2.listener->wait_for_data();
          //     //std::cout <<"id2:" << h264Stream2.id() << "---"<<h264Stream2.name()<<" --- "<<h264Stream2.channelNumber() <<std::endl;
          //     mDoMsg(h264Stream2);
          //   }
          // }
// private:
//     std::thread myThread;
};
*/


int main() {

  // Create participant. Arguments-> Domain id, QOS name
  //ShmParticipant dp;
  //Msg msg;
  //Msg msg2;

  //ShmParticipant dp2;

  //dp.init(AODS_I7_MAIN_0, "hi");

  //dp2.init(AODS_I7_MAIN_0, "hi");

  // Message
  // idl_msg::Position pos_msg{};
  //vis_msg::ObjectByte objectByte{};
  //vis_msg::H264Stream h264Stream{};

  // Create subscriber with msg type
  //DDSSubscriber h264_sub(vis_msg::ObjectBytePubSubType(), objectByte, AODS_1126_OBJ_BYTE_L,dp.participant());

  //DDSSubscriber h264_sub2(vis_msg::H264StreamPubSubType(),h264Stream2, AODS_1126_OBJ_L,dp2.participant());

  //h264_sub.setMsgCallback(std::bind(&Msg::threadFunction, msg, std::placeholders::_1));
  //h264_sub2.setMsgCallback(std::bind(&Msg::threadFunction, msg, std::placeholders::_1));
  //msg.start();

  //std::thread myThread(std::bind(&Msg::setMsgCallback, msg, std::placeholders::_1),std::bind(&Msg::threadFunction, msg, std::placeholders::_1));

  //DDSSubscriber<vis_msg::H264StreamPubSubType,vis_msg::H264Stream> h264_sub;

  //h264_sub.init(h264Stream, AODS_1126_H264_L,dp.participant());
    ShmParticipant dp;

    //DDSSubscriber<vis_msg::H264StreamPubSubType, vis_msg::H264Stream>  h264Sub;
    DDSSubscriber<vis_msg::ObjectBytePubSubType, vis_msg::ObjectByte> h264Sub;

    vis_msg::ObjectByte mh264Stream;

    if(dp.init(AODS_I7_MAIN_0, "hi88"))
    {
        printf("dp OK! \r\n");
    }
    else
    {
        printf("dp Fail! \r\n");
    }


    if (h264Sub.init(mh264Stream, AODS_1126_OBJ_BYTE_L, dp.participant()))
    {
        printf("Sub OK! \r\n");
    }
    else
    {
        printf("Sub Fail! \r\n");
    }
    printf("------------------1234567890------------------- \r\n");

  for (;;) {

    // Blocks until new data is available
     // h264Sub.listener->wait_for_data();
    // std::cout <<"id:" << h264Stream.id() << "---"<<h264Stream.name()<<" --- "<<h264Stream.channelNumber() <<std::endl;
     if(h264Sub.listener->wait_for_data_for_ms(10))
     {
           //printf("-------------------------id:%d --------------------\r\n", mh264Stream.id());
           std::cout <<"id:" << mh264Stream.id() <<" --- "<< mh264Stream.channelNumber() <<std::endl;
     }
     //printf("!!!!!!!!!!!!!!!!!!!! \r\n");
     
    // if(h264_sub2.listener->wait_for_data_for_ms(1))
    // {
    //   std::cout <<"id2:" << h264Stream2.id() << "---"<<h264Stream2.name()<<" --- "<<h264Stream2.channelNumber() <<std::endl;
    // }
    //std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  }
}
